#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_1,    servo7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo8,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    servo9,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo10,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "rdpartyrobotcdr-v2.2\drivers\HTSMUX-driver.h"
#include "rdpartyrobotcdr-v2.2\drivers\HTMAG-driver.h"
#include "rdpartyrobotcdr-v2.2\drivers\LEGOLS-driver.h"

const float armSeg1 = 15.25;
const float armSeg2 = 11;
const tMUXSensor HTMAG = msensor_S2_1;
const tMUXSensor ballLS = msensor_S2_2;
int magballcount = 0;
float armTarget[] = {0,0};
float armAngles[] = {0,0,0};
float wristTarget[] = {0,0};
float wristOffset[] = {0,0};

void initArm(){
  nMotorEncoder[motorF] = 0;
}

void runArm(int targetAngle, int motorNumber){
  nMotorEncoderTarget[motorF] = targetAngle;// - nMotorEncoder[motorF];
  if((targetAngle - nMotorEncoder[motorF])<0){
    motor[motorF] = -20;
  }else if((targetAngle - nMotorEncoder[motorF])>0){
    motor[motorF] = 20;
  }
}

float atan2(float arg0,float arg1){
  if(arg1>0){
    return atan(arg0/arg1);
  }else if(arg0>=0&&arg1<0){
    return atan(arg0/arg1)+PI;
  }else if(arg0<0&&arg1<0){
    return atan(arg0/arg1)-PI;
  }else if(arg0>0&&arg1==0){
    return PI/2;
  }else if(arg0<0&&arg1==0){
    return -(PI/2);
  }else{
    return 0;
  }
}

void calibrate()
{
  servoChangeRate[servo1] = 10;
  servoChangeRate[servo2] = 1;
  motor[motorG] = -25;
  wait1Msec(500);
  HTMAGstartCal(HTMAG);
  wait1Msec(1000);
  servo[servo2] = 255;
  motor[motorG] = 0;
  wait1Msec(500);
  LSsetActive(ballLS);
  return;
}

void initializeRobot()
{
  servo[servo1] = 140;
  servoChangeRate[servo1] = 0;
  servoChangeRate[servo2] = 2;
  servo[servo6] = 0;
  servoChangeRate[servo8] = 1;
  servo[servo5] = 0;
  servoChangeRate[servo7] = 1;
  wait1Msec(300);
  calibrate();
  nMotorEncoder[motorF] = 0;
}

task arm()
{
  initArm();
  float c2;
  float s2;
  float k1;
  float k2;
  bool waitWrist = false;
  while(true)
  {
    hogCPU();
    getJoystickSettings(joystick);
    if (!joystick.StopPgm && !((joystick.joy1_Buttons & (1 << 9)) != 0) && !((joystick.joy2_Buttons & (1 << 9)) != 0))
    {
      if((sqrt(joystick.joy2_x1^2 + joystick.joy2_y1^2) > 100) && !waitWrist)
      {
        armTarget[0] += joystick.joy2_x1 * -0.002;
        armTarget[1] += joystick.joy2_y1 * 0.002;
      }
      else if(!(sqrt(joystick.joy2_x1^2 + joystick.joy2_y1^2) > 100) && waitWrist)
      {
        armTarget[0] = 0;
        armTarget[1] = 0;
      }
      if(sqrt(joystick.joy2_x2^2 + joystick.joy2_y2^2) > 2500)
      {
        wristTarget[0] = joystick.joy2_x2;
        wristTarget[1] = joystick.joy2_y2;
      }
      else
      {
        wristTarget[0] = 0;
        wristTarget[1] = 0;
      }
    }
    if((armSeg1 + armSeg2) <= sqrt(pow(armTarget[0],2) + pow(armTarget[1],2)))
    {
      armTarget[0] -= 0.25 * armTarget[0]/sqrt(pow(armTarget[0],2) + pow(armTarget[1],2));
      armTarget[1] -= 0.25 * armTarget[1]/sqrt(pow(armTarget[0],2) + pow(armTarget[1],2));
    }
    if(armTarget[0]!=0&&armTarget[1]!=0){
	    if((armSeg1 - armSeg2) >= sqrt(pow(armTarget[0],2) + pow(armTarget[1],2)))
	    {
	      armTarget[0] += 0.25 * armTarget[0]/sqrt(pow(armTarget[0],2) + pow(armTarget[1],2));
	      armTarget[1] += 0.25 * armTarget[1]/sqrt(pow(armTarget[0],2) + pow(armTarget[1],2));
	    }
	  }
    else{
      armTarget[0] = 1;
      armTarget[1] = 1;
    }
    c2 = (pow(armTarget[0],2) + pow(armTarget[1],2) - pow(armSeg1,2) - pow(armSeg2,2))/(2*armSeg1*armSeg2);
    s2 = sqrt(1 - pow(c2,2));
    k1 = armSeg1 + armSeg2 * c2;
    k2 = armSeg2 * s2;
    armAngles[0] = atan2(armTarget[1],armTarget[0])-atan2(k2,k1);
    armAngles[1] = atan2(s2,c2);
    armAngles[2] = atan2(wristTarget[1],wristTarget[0]);
    servo[servo8] = (256 * armAngles[1] / PI);
    servo[servo7] = (256 * armAngles[1] / PI);
    motor[motorB] = (radiansToDegrees(armAngles[2]) - nMotorEncoder[motorB] + wristOffset[0]);
    motor[motorC] = (radiansToDegrees(armAngles[2]) - nMotorEncoder[motorC] + wristOffset[1]);
    releaseCPU();
    if (joystick.joy2_TopHat == 2)
    {
      servo[servo9] = 127;
      servo[servo10] = 127;
    }
    if (joystick.joy2_TopHat == 4)
    {
      servo[servo9] = 0;
      servo[servo10] = 255;
    }
    if (joystick.joy2_TopHat == 6)
    {
      servo[servo9] = 255;
      servo[servo10] = 0;
    }
    float armMotorAngle = (radiansToDegrees(armAngles[0])*4)*10;
    runArm(armMotorAngle, motorF);
    wait1Msec(10);
  }
}

task ballCollection()
{
  //int lightcount = 0;
  int magcount = 0;
  //bool waitconveyor = false;
  while(false)
  {
    getJoystickSettings(joystick);
    if (!joystick.StopPgm && !((joystick.joy1_Buttons & (1 << 9)) != 0) && !((joystick.joy2_Buttons & (1 << 9)) != 0))
    {
	    motor[motorG] = -25;
		  if (abs(HTMAGreadVal(HTMAG)) > 3)
		  {
		    if (time1[T1] > 10)
		    {
			    ++magcount;
			    ClearTimer(T1);
			  }
		  }
	    else
		  {
		    magcount = 0;
		  }
		  if (magcount > 3)
		  {
		    motor[motorG] = 0;
		    servo[servo1] = 196;
		    wait1Msec(1000);
		    motor[motorG] = -20;
		    wait1Msec(1500);
		    motor[motorG] = 0;
		    wait1Msec(1000);
		    magballcount = 0;
		    motor[motorG] = -25;
		  }
		  else
		  {
		    servo[servo1] = 22;
		  }
		  if (magballcount == 2)
		  {
		    servo[servo2] = 140;
		    magballcount = 0;
		    ClearTimer(T2);
		  }
		  if (time1[T2] > 10000)
		  {
		    servo[servo2] = 255;
		  }
    }
    if ((joystick.joy2_Buttons & (1 << 8)) != 0)
    {
      calibrate();
    }
    //test code. remove.
    motor[motorG] = 0;
    if (time1[T1] >= 1000 && time1[T1] <= 1020)
      servo[servo1] = 196;
    if (joystick.joy1_TopHat == 2)
      servo[servo1] = 32;
    if (joystick.joy1_TopHat == 6)
      servo[servo1] = 196;
    if (joystick.joy1_TopHat == 4)
      servo[servo1] = 122;
    if ((joystick.joy1_Buttons & (1 << 3)) != 0)
      servo[servo2] = 255;
    if ((joystick.joy1_Buttons & (1 << 1)) != 0)
      servo[servo2] = 110;
    if ((joystick.joy1_Buttons & (1 << 0)) != 0)
      servo[servo2] = 140;
    wait1Msec(10);
  }
}

task main()
{
  initializeRobot();
  float speed = 1;
  int dir = 1;
  bool waitdir = false;
  bool waitspeed = false;
  waitForStart();
  ClearTimer(T3);
  StartTask(arm);
  StartTask(ballCollection);
  while(true)
  {
    getJoystickSettings(joystick);
    if (((joystick.joy1_Buttons & (1 << 5)) != 0) && !waitspeed)
    {
      if (speed == 1)
      {
        speed = 0.5;
        waitspeed = true;
      }
      else if (speed == 0.5)
      {
        speed = 1;
        waitspeed = true;
      }
    }
    else if (!((joystick.joy1_Buttons & (1 << 5)) != 0) && waitspeed)
    {
      waitspeed = false;
    }

    if (((joystick.joy1_Buttons & (1 << 7)) != 0) && !waitdir)
    {
      if (dir == 1)
      {
        dir = -1;
        waitdir = true;
      }
      else
      {
        dir = 1;
        waitdir = true;
      }
    }
    else if (!((joystick.joy1_Buttons & (1 << 7)) != 0) && waitdir)
    {
      waitdir = false;
    }
    if (!joystick.StopPgm && !((joystick.joy1_Buttons & (1 << 9)) != 0) && !((joystick.joy2_Buttons & (1 << 9)) != 0))
    {
      if (dir == 1)
      {
        if (joystick.joy1_y1 > -10 && joystick.joy1_y1 < 10)
        {
          motor[motorD] = 0;
        }
        if (joystick.joy1_y1 >= 10 && joystick.joy1_y1 < 50)
        {
          motor[motorD] = dir * speed * (0.5*joystick.joy1_y1 - 5);
        }
        if(joystick.joy1_y1 <= -10 && joystick.joy1_y1 > -50)
        {
          motor[motorD] = dir * speed * (0.5*joystick.joy1_y1 + 5);
        }
        if (joystick.joy1_y1 >= 50 && joystick.joy1_y1 < 100)
        {
          motor[motorD] = dir * speed * (1.3*joystick.joy1_y1 - 40);
        }
        if (joystick.joy1_y1 <= -50 && joystick.joy1_y1 > -100)
        {
          motor[motorD] = dir * speed * (1.3*joystick.joy1_y1 + 40);
        }
        if (joystick.joy1_y1 >= 100 && joystick.joy1_y1 < 129)
        {
          motor[motorD] = dir * speed * (0.37*joystick.joy1_y1 + 52.64);
        }
        if (joystick.joy1_y1 <= -100 && joystick.joy1_y1 >= -129)
        {
          motor[motorD] = dir * speed * (0.37*joystick.joy1_y1 -52.64);
        }
        if (joystick.joy1_y2 > -10 && joystick.joy1_y2 < 10)
        {
          motor[motorE] = 0;
        }
        if (joystick.joy1_y2 >= 10 && joystick.joy1_y2 < 50)
        {
          motor[motorE] = dir * speed * (0.5*joystick.joy1_y2 - 5);
        }
        if(joystick.joy1_y2 <= -10 && joystick.joy1_y2 > -50)
        {
          motor[motorE] = dir * speed * (0.5*joystick.joy1_y2 + 5);
        }
        if (joystick.joy1_y2 >= 50 && joystick.joy1_y2 < 100)
        {
          motor[motorE] = dir * speed * (1.3*joystick.joy1_y2 - 40);
        }
        if (joystick.joy1_y2 <= -50 && joystick.joy1_y2 > -100)
        {
          motor[motorE] = dir * speed * (1.3*joystick.joy1_y2 + 40);
        }
        if (joystick.joy1_y2 >= 100 && joystick.joy1_y2 < 129)
        {
          motor[motorE] = dir * speed * (0.37*joystick.joy1_y2 + 52.64);
        }
        if (joystick.joy1_y2 <= -100 && joystick.joy1_y2 >= -129)
        {
          motor[motorE] = dir * speed * (0.37*joystick.joy1_y2 -52.64);
        }
      }
      else if (dir == -1)
      {
        if (joystick.joy1_y1 > -10 && joystick.joy1_y1 < 10)
        {
          motor[motorE] = 0;
        }
        if (joystick.joy1_y1 >= 10 && joystick.joy1_y1 < 50)
        {
          motor[motorE] = dir * speed * (0.5*joystick.joy1_y1 - 5);
        }
        if(joystick.joy1_y1 <= -10 && joystick.joy1_y1 > -50)
        {
          motor[motorE] = dir * speed * (0.5*joystick.joy1_y1 + 5);
        }
        if (joystick.joy1_y1 >= 50 && joystick.joy1_y1 < 100)
        {
          motor[motorE] = dir * speed * (1.3*joystick.joy1_y1 - 40);
        }
        if (joystick.joy1_y1 <= -50 && joystick.joy1_y1 > -100)
        {
          motor[motorE] = dir * speed * (1.3*joystick.joy1_y1 + 40);
        }
        if (joystick.joy1_y1 >= 100 && joystick.joy1_y1 < 129)
        {
          motor[motorE] = dir * speed * (0.37*joystick.joy1_y1 + 52.64);
        }
        if (joystick.joy1_y1 <= -100 && joystick.joy1_y1 >= -129)
        {
          motor[motorE] = dir * speed * (0.37*joystick.joy1_y1 -52.64);
        }
        if (joystick.joy1_y2 > -10 && joystick.joy1_y2 < 10)
        {
          motor[motorD] = 0;
        }
        if (joystick.joy1_y2 >= 10 && joystick.joy1_y2 < 50)
        {
          motor[motorD] = dir * speed * (0.5*joystick.joy1_y2 - 5);
        }
        if(joystick.joy1_y2 <= -10 && joystick.joy1_y2 > -50)
        {
          motor[motorD] = dir * speed * (0.5*joystick.joy1_y2 + 5);
        }
        if (joystick.joy1_y2 >= 50 && joystick.joy1_y2 < 100)
        {
          motor[motorD] = dir * speed * (1.3*joystick.joy1_y2 - 40);
        }
        if (joystick.joy1_y2 <= -50 && joystick.joy1_y2 > -100)
        {
          motor[motorD] = dir * speed * (1.3*joystick.joy1_y2 + 40);
        }
        if (joystick.joy1_y2 >= 100 && joystick.joy1_y2 < 129)
        {
          motor[motorD] = dir * speed * (0.37*joystick.joy1_y2 + 52.64);
        }
        if (joystick.joy1_y2 <= -100 && joystick.joy1_y2 >= -129)
        {
          motor[motorD] = dir * speed * (0.37*joystick.joy1_y2 -52.64);
        }
      }
    }
    else
    {
      motor[motorD] = 0;
      motor[motorE] = 0;
      motor[motorG] = 0;
    }
    wait1Msec(10);
  }

}
